/*
 * @Author       : Zou Kai
 * @Date         : 2024-10-30 18:57:45
 * @Last Modified by: Your Name
 * @Last Modified time: 
 * @Description  : 
 */

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <Eigen/Dense>
#include<opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/core/eigen.hpp>
#include "yaml-cpp/yaml.h"
#include "common.h"
#include "square_projection.h"

int main(int argc, char const *argv[]){

    std::string lidar_file_path = argv[1];
    // std::string image_file_path = argv[2];
 
    std::string config_path = "./configs/configs.yaml";
    // 1.加载数据
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    if(pcl::io::loadPCDFile<pcl::PointXYZI>(lidar_file_path, *cloud) == -1){
        PCL_ERROR("Couldn't read file lidar.pcd \n");
        return -1;
    }
    // cv::Mat image = cv::imread(image_file_path);
    // if(image.empty()){
    //     std::cerr<<"Failed to open image file"<<std::endl;
    //     return -1;
    // }

    YAML::Node work_config = YAML::LoadFile(config_path);
    if(!work_config){
        std::cerr<<"Failed to open settings file at :"
        <<config_path<<std::endl;
        return -1;
    };
    std::string intrinsic_file_path = work_config["intrinsic_file"].as<std::string>();
    std::string extrinsic_file_path = work_config["extrinsic_file"].as<std::string>();
    std::string camera_name = work_config["device_name"].as<std::string>();
    int project_model = work_config["project_model"].as<int>();
    float cerr_loss = work_config["cerr_loss"].as<float>();

    std::shared_ptr<SquareProject> squareproject;
    squareproject.reset(new SquareProject());
    if(!squareproject->Init(extrinsic_file_path, intrinsic_file_path, camera_name, project_model, cerr_loss)){
        std::cerr<<"Failed to init "<<std::endl;
        return -1;
    };

    // 2.将点云从lidar坐标系转换到相机坐标系
    pcl::transformPointCloud(*cloud, *cloud, squareproject->ProjectConfig_.T_cam_to_lidar_.inverse().eval());
    pcl::io::savePCDFileASCII("lidar_to_cam.pcd", *cloud);

    //3.取像素值里面最近点
    squareproject->FarthestPointPixel(cloud);
    // std::cout<<squareproject->ProjectConfig_.pixel_grid.size()<<std::endl;
    //4.计算每个像素值中的误差范围square_vec
    std::map<int,std::vector<cv::Point2d>> square_vec;
    square_vec = squareproject->ErrorProject();

    return 0;
}